from matplotlib import pyplot as plt
from matplotlib import animation
import numpy as np
from IPython.display import HTML
%matplotlib notebook
from kinematics import Vector, Quaternion, Transform
import graphics
Манипулятор кинематической схемы SCARA обладает четыремя степенями подвижности.

Можно выделить высоту колонны, длины первого и второго звеньев.
Обобщенные координаты будут в радианах и метрах:
| Обобщенная координата | Обозначение | Размерность |
|---|---|---|
| Вращение вокруг колонны | $q_0$ | радиан |
| Вращение в локте | $q_1$ | радиан |
| Вращение инструмента | $q_2$ | радиан |
| Перемещение инструмента | $q_3$ | метр |
def scara_chain(q, l, math_source=np):
base = Transform.identity()
column = base + Transform(
Vector(0, 0, l[0]),
Quaternion.from_angle_axis(q[0], Vector(0, 0, 1), math_source)
)
elbow = column + Transform(
Vector(l[1], 0, 0),
Quaternion.from_angle_axis(q[1], Vector(0, 0, 1), math_source)
)
tool = elbow + Transform(
Vector(l[2], 0, 0),
Quaternion.from_angle_axis(q[2], Vector(0, 0, 1), math_source)
)
flange = tool + Transform(
Vector(0, 0, -q[3]),
Quaternion.identity()
)
return [
base,
column,
elbow,
tool,
flange
]
Зададим закон изменения обобщенных координат:
def scara_q(t, total):
#omega = t / total * np.pi * 2
a = (t - total) / 2
omega = (-np.cos(3.14 / total * t) + 1.0)*5
return [
np.pi / 2 * np.sin(omega)-np.pi,
np.pi / 2*np.cos(-omega),
omega,
3 + 3 * np.cos(omega)
]
Укажем длины звеньев:
scara_l = [8, 4, 3]
scara_fig = plt.figure()
ax = scara_fig.add_subplot(projection="3d")
ax.set_xlim(-6, 6); ax.set_ylim(-6, 6); ax.set_zlim(0, 12)
lines, = ax.plot([], [], [], color="#000000")
graphics.axis(ax, Transform.identity(), 2)
r, g, b = graphics.axis(ax, Transform.identity(), 1)
total = 100
def animate(frame):
chain = scara_chain(scara_q(frame, total), scara_l)
(x, y, z) = graphics.chain_to_points(chain)
lines.set_data_3d(x, y, z)
global r, g, b
r.remove(); g.remove(); b.remove()
r, g, b = graphics.axis(ax, chain[-1], 0.5)
animate(0)
fps = 25
scara_ani = animation.FuncAnimation(
scara_fig,
animate,
frames=total,
interval=1000.0/fps
)
HTML(scara_ani.to_jshtml())